﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class MBLSimpleGradientSelfLocator : SelfLocator
  {
    WorldState randomStart;
    Type currentWorldStateType = null;
    int currentIndex = 0;

    GradientDescentTool gradientDescentTool = new GradientDescentTool();
    
    public MBLSimpleGradientSelfLocator()
    {
      randomStart = null;
    }

    public override WorldState Execute(Experiment experiment)
    {
//      if (!DebugKeyTool.IsActive("MBLSimpleGradientSelfLocator:freeze random position"))
      {
        if (
          randomStart == null || 
          currentWorldStateType != experiment.simulatedWorldState.GetType() ||
          DebugKeyTool.IsActive("MBLSimpleGradientSelfLocator:init random position")
          )
        {
          currentWorldStateType = experiment.simulatedWorldState.GetType();
          if (experiment.simulatedWorldState is WorldState1D)
          {
            randomStart = new WorldState1D();
          }
          else if (experiment.simulatedWorldState is WorldState2D)
          {
            randomStart = new WorldState2D();
          }
          else if (experiment.simulatedWorldState is WorldStatePose2D)
          {
            randomStart = new WorldStatePose2D();
          }
        }
      }
      if (experiment.actionAndObservationHistory.history.Count < currentIndex)
      {
        currentIndex = 0;
      }
      while (currentIndex < experiment.actionAndObservationHistory.history.Count)
      {
        if (experiment.actionAndObservationHistory.history[currentIndex] is ActionDescription)
        {
          ActionDescription currentAction = experiment.actionAndObservationHistory.history[currentIndex] as ActionDescription;
          randomStart = currentAction.ExecutePerfect(experiment.motionModel, randomStart);
        }
        currentIndex++;
      }
      if (DebugKeyTool.IsActive("GD:reset with real pose")) randomStart = (WorldState)experiment.simulatedWorldState.Clone();
      //randomStart 
      WorldState result;
      result = gradientDescentTool.DoGradientDescent2(randomStart, experiment);
      randomStart = result;
//      if (DebugKeyTool.IsActive("MBLSimpleGradientSelfLocator:intelligent version"))
//        result = MBLTools.DoIntelligentGradientDescent(randomPosition, experiment);
      return result;
    }
  }
}
